/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.BallPickupStop;

/**
 *
 * @author Eddie
 */
public class BallPickup extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    
    Jaguar leftPickupMotor_Lower;
    Jaguar rightPickupMotor_Lower;
    Jaguar pickupMotor_Upper;

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
        setDefaultCommand(new BallPickupStop());
    }
    
    public BallPickup() {
        super("BallPickup");
        
        leftPickupMotor_Lower = new Jaguar(RobotMap.PICKUP_LEFT_LOWER);
        rightPickupMotor_Lower = new Jaguar(RobotMap.PICKUP_RIGHT_LOWER);
        pickupMotor_Upper = new Jaguar(RobotMap.PICKUP_UPPER);

    }
    
    public void pickupBall() {
        int leftMultiplier = 1;
        int rightMultiplier = 1;
        int upperMultiplier = 1;

        if(RobotMap.PICKUP_LEFT_REVERSED) {
            leftMultiplier = -1;
        }

        if(RobotMap.PICKUP_RIGHT_REVERSED) {
            rightMultiplier = -1;
        }

        if(RobotMap.PICKUP_UPPER_REVERSED) {
            upperMultiplier = -1;
        }

        // Use the speed found in robot map and whether or not the motor should be reflected
        // To set to pickup motor values
        leftPickupMotor_Lower.set(RobotMap.PICKUP_MAX_SPEED * leftMultiplier);
        rightPickupMotor_Lower.set(RobotMap.PICKUP_MAX_SPEED * rightMultiplier);
        pickupMotor_Upper.set(RobotMap.PICKUP_MAX_SPEED * upperMultiplier);
    }

    public void pickupLower() {
        int leftMultiplier = 1;
        int rightMultiplier = 1;

        if(RobotMap.PICKUP_LEFT_REVERSED) {
            leftMultiplier = -1;
        }

        if(RobotMap.PICKUP_RIGHT_REVERSED) {
            rightMultiplier = -1;
        }

        // Use the speed found in robot map and whether or not the motor should be reflected
        // To set to pickup motor values
        leftPickupMotor_Lower.set(RobotMap.PICKUP_MAX_SPEED * leftMultiplier);
        rightPickupMotor_Lower.set(RobotMap.PICKUP_MAX_SPEED * rightMultiplier);
    }

    public void pickupUpper() {
        int upperMultiplier = 1;

        if(RobotMap.PICKUP_UPPER_REVERSED) {
            upperMultiplier = -1;
        }

        // Use the speed found in robot map and whether or not the motor should be reflected
        // To set to pickup motor values
        pickupMotor_Upper.set(RobotMap.PICKUP_MAX_SPEED * upperMultiplier);
    }

    public void reverseLower() {
        int leftMultiplier = 1;
        int rightMultiplier = 1;

        if(RobotMap.PICKUP_LEFT_REVERSED) {
            leftMultiplier = -1;
        }

        if(RobotMap.PICKUP_RIGHT_REVERSED) {
            rightMultiplier = -1;
        }

        // Use the speed found in robot map and whether or not the motor should be reflected
        // To set to pickup motor values
        leftPickupMotor_Lower.set(-(RobotMap.PICKUP_MAX_SPEED * leftMultiplier));
        rightPickupMotor_Lower.set(-(RobotMap.PICKUP_MAX_SPEED * rightMultiplier));
    }

    public void reverseUpper() {
        int upperMultiplier = 1;

        if(RobotMap.PICKUP_UPPER_REVERSED) {
            upperMultiplier = -1;
        }

        // Use the speed found in robot map and whether or not the motor should be reflected
        // To set to pickup motor values
        pickupMotor_Upper.set(-(RobotMap.PICKUP_MAX_SPEED * upperMultiplier));
    }
    
    public void reverse() {
        int leftMultiplier = 1;
        int rightMultiplier = 1;
        int upperMultiplier = 1;
        
        if(RobotMap.PICKUP_LEFT_REVERSED) {
            leftMultiplier = -1;
        }
        
        if(RobotMap.PICKUP_RIGHT_REVERSED) {
            rightMultiplier = -1;
        }

        if(RobotMap.PICKUP_UPPER_REVERSED) {
            upperMultiplier = -1;
        }
        
        // Use the speed found in robot map and whether or not the motor should be reflected 
        // To set to pickup motor values
        leftPickupMotor_Lower.set(-(RobotMap.PICKUP_MAX_SPEED * leftMultiplier));
        rightPickupMotor_Lower.set(-(RobotMap.PICKUP_MAX_SPEED * rightMultiplier));
        pickupMotor_Upper.set(-(RobotMap.PICKUP_MAX_SPEED * upperMultiplier));
    }
    
    public void stopMotors() {
        leftPickupMotor_Lower.set(0.0); //
        rightPickupMotor_Lower.set(0.0);
        pickupMotor_Upper.set(0.0); //
    }
}
